#!/usr/bin/env python3
# -*- coding: UTF-8 -*-

import time
import rospy

from geometry_msgs.msg import PointStamped,Pose,PoseStamped
from nav_msgs.msg import Path
from v3_msgs.msg import *
from v3_msgs.srv import *

class FullPathType:
    BOUSTROPHEDON = 0
    BOUSTROPHEDON_AND_EDGE = 1
    WRAPPER = 2
    WRAPPER_AND_EDGE = 3
    EDGE = 4
    

class CCPP_Test:
    def __init__(self):
        rospy.init_node("test_ccpp_node")
        self.__start = None
        self.__polygon = None 
        self.__path_gen_srv = rospy.ServiceProxy("/cover_path_server", PathGenerateManager)
        self.__path_pub = rospy.Publisher("/ccpp_path", Path, queue_size=1, latch=True)
        self.__edge_path_pub = rospy.Publisher("/ccpp_edge_path", Path, queue_size=1, latch=True)
        rospy.Subscriber("/clicked_point", PointStamped, callback=self.ClickedPointCb)
        self.Test()

    def ClickedPointCb(self, msg):
        pose_msg = Pose()
        pose_msg.position = msg.point
        print("[ClickedPointCb] x={}, y={}, z={}".format(
            msg.point.x, msg.point.y, msg.point.z
        ))
        if self.__start is None:
            self.__start = pose_msg
            print("--> set start point.")
        else:
            if self.__polygon is None:
                self.__polygon = PolygonData()
                self.__polygon.poses.append(pose_msg)
                print("--> add first polygon point. size={}".format(len(self.__polygon.poses)))
            else:
                self.__polygon.poses.append(pose_msg)
                print("--> add first polygon point. size={}".format(len(self.__polygon.poses)))

    def FullPathType2Str(self, path_type):
        if path_type == FullPathType.BOUSTROPHEDON:
            return "BOUSTROPHEDON"
        elif path_type == FullPathType.WRAPPER:
            return "WRAPPER"
        elif path_type == FullPathType.BOUSTROPHEDON_AND_EDGE:
            return "BOUSTROPHEDON_AND_EDGE"
        elif path_type == FullPathType.WRAPPER_AND_EDGE:
            return "WRAPPER_AND_EDGE"
        elif path_type == FullPathType.EDGE:
            return "EDGE"
        return ""

    def GenPathType(type):
        path_type = PathGenerateType()
        path_type.type = type
        return path_type

    def PathGenMsg(self, path_types):
        path_gen_msg = PathGenerate()
        path_gen_msg.start = self.__start
        path_gen_msg.polygon = self.__polygon
        path_gen_msg.path_types = path_types
        # path_gen_msg.path_types_child = path_types_child
        return path_gen_msg


    def FullPathGen(self, path_type):
        '''路径生成测试'''
        
        def GenPathType(type):
            '''
                生成路径类型
            '''
            path_type = PathGenerateType()
            path_type.type = type
            return path_type

        try:
            if not self.__start:
                raise Exception("self.__start empty!")
            if not self.__polygon:
                raise Exception("self.__polygon empty!")

            client_data = PathGenerateManagerRequest()
            client_data.option = client_data.GENERATE_DEFAULT
            client_data.is_preview = False
            client_data.task_speed.speed = TaskSpeed.LOW_SPEED
            # int16 UNKNOWN = 100                   #未知
            # int16 SWEEP_STANDARD_MODE = 0         #标准清扫模式
            # int16 SWEEP_EDGE_MODE = 10            #贴边清扫模式
            # int16 SWEEP_STANDARD_AND_EDGE = 11    #标准+贴边清扫模式
            # int16 SWEEP_STANDARD_AND_REPAIR = 21  #标准+补漏清扫模式
            # int16 FOLLOW_MODE = 31                  #跟随模式    
            control_mode = TaskControlMode()
            path_types = list()
            if path_type == FullPathType.BOUSTROPHEDON:
                control_mode.mode = TaskControlMode.SWEEP_STANDARD_MODE
                path_types.append(GenPathType(PathGenerateType.BOUSTROPHEDON_METHOD))
                print("SWEEP_STANDARD_MODE | BOUSTROPHEDON_METHOD")
            elif path_type == FullPathType.WRAPPER:
                control_mode.mode = TaskControlMode.SWEEP_STANDARD_MODE
                path_types.append(GenPathType(PathGenerateType.WRAPPER_METHOD))
                print("SWEEP_STANDARD_MODE | WRAPPER_METHOD")
            elif path_type == FullPathType.BOUSTROPHEDON_AND_EDGE:
                control_mode.mode = TaskControlMode.SWEEP_STANDARD_AND_EDGE
                path_types.append(GenPathType(PathGenerateType.BOUSTROPHEDON_METHOD))
                path_types.append(GenPathType(PathGenerateType.EDGE_METHOD))
                print("SWEEP_STANDARD_AND_EDGE | BOUSTROPHEDON_METHOD | EDGE_METHOD")
            elif path_type == FullPathType.WRAPPER_AND_EDGE:
                control_mode.mode = TaskControlMode.SWEEP_STANDARD_AND_EDGE
                path_types.append(GenPathType(PathGenerateType.WRAPPER_METHOD))
                path_types.append(GenPathType(PathGenerateType.EDGE_METHOD))
                print("SWEEP_STANDARD_AND_EDGE | WRAPPER_METHOD | EDGE_METHOD")
            elif path_type == FullPathType.EDGE:
                control_mode.mode = TaskControlMode.SWEEP_EDGE_MODE
                path_types.append(GenPathType(PathGenerateType.EDGE_METHOD))
                print("SWEEP_EDGE_MODE | EDGE_METHOD")

            client_data.control_mode = control_mode
            client_data.path_gen = self.PathGenMsg(path_types)
            res_data = self.__path_gen_srv.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
            print("[FullPathGen] paths size={}, edge_paths={}, estimated_time={}, task_area={}".format(
                len(res_data.paths), len(res_data.edge_paths), res_data.estimated_time, res_data.task_area))
            
            # 普通路径生成
            path_msg = Path()
            path_msg.header.frame_id = "map"
            path_msg.header.stamp = rospy.Time.now()
            for task_path in res_data.paths:
                for pose in task_path.poses:
                    pose_stamp = PoseStamped()
                    pose_stamp.pose = pose
                    path_msg.poses.append(pose_stamp)
            self.__path_pub.publish(path_msg)

            # 贴边路径生成
            edge_path_msg = Path()
            edge_path_msg.header.frame_id = "map"
            edge_path_msg.header.stamp = rospy.Time.now()
            for task_path in res_data.edge_paths:
                for pose in task_path.poses:
                    pose_stamp = PoseStamped()
                    pose_stamp.pose = pose
                    edge_path_msg.poses.append(pose_stamp)
            self.__edge_path_pub.publish(edge_path_msg)
        except Exception as error:
            rospy.logerr("[PathGen] error: {}".format(error))

    def Test(self):
        while not rospy.is_shutdown():
            time.sleep(0.1)
            print("---------Test--------")
            print("0 - 弓字形")
            print("1 - 弓字形+贴边")
            print("2 - 回字形")
            print("3 - 回字形+贴边")
            print("4 - 贴边")
            print("10 - 清空起点和多边形轮廓")
            print("11 - 退出")
            cmd = input("请输入对应操作指令:\n")
            cmd = int(cmd)
            if cmd == 0:
                self.FullPathGen(FullPathType.BOUSTROPHEDON)
            elif cmd == 1:
                self.FullPathGen(FullPathType.BOUSTROPHEDON_AND_EDGE)
            elif cmd == 2:
                self.FullPathGen(FullPathType.WRAPPER)
            elif cmd == 3:
                self.FullPathGen(FullPathType.WRAPPER_AND_EDGE)
            elif cmd == 4:
                self.FullPathGen(FullPathType.EDGE)
            elif cmd == 10:
                self.__start = None
                self.__polygon = None 
            elif cmd == 11:
                rospy.signal_shutdown("closed!")
            else:
                print("请输入正确指令")

if __name__ == "__main__":
    CCPP_Test()
    rospy.spin()